/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.techhighteam675.robot2012;

/**
 *
 * @author TJ
 */
public final class Definitions {
    
    // Send PID Controllers and Target Data to SmartDashboard
    
    public static final boolean DEBUG_MODE = true;

    /*****************************************************************
    *                             PORTS                              *
    *****************************************************************/

    // Devices
    public static final boolean IO_CHIP_ATTACHED = false;

    //Module Definitions
    public static final int ANALOG_MODULE = 1;
    public static final int DRIVE_DIGITAL_MODULE = 1;
    public static final int PAYLOAD_DIGITAL_MODULE = 2;
    public static final int SOLENOID_MODULE = 1;

    //Digital Drive Ports (On Module 1)
            //PWMs
    public static final int LEFT_VICTOR_1 = 1;
    public static final int LEFT_VICTOR_2 = 2;
    public static final int RIGHT_VICTOR_1 = 3;
    public static final int RIGHT_VICTOR_2 = 4;
            //Digital I/O
    public static final int LEFT_ENCODER_A = 1;
    public static final int LEFT_ENCODER_B = 2;
    public static final int RIGHT_ENCODER_A = 3;
    public static final int RIGHT_ENCODER_B = 4;
    public static final int COMPRESSOR_SWITCH = 5;
            //Relays
    public static final int COMPRESSOR_CHANNEL = 1;

    //Digital Payload Ports (On Module 2)
            //PWMs
    public static final int BELT_MOTOR_PICKUP = 4;
    public static final int THETA_VIC = 5;
    public static final int BELT_MOTOR_SHOOTING = 1;
    public static final int SPEED_VIC_1 = 2;
    public static final int SPEED_VIC_2 = 3;
    public static final int PHI_VIC = 6;
    
            //Digital I/O
    public static final int PHI_ENCODER_A = 3;
    public static final int PHI_ENCODER_B = 4;
    public static final int SPEED_ENCODER_A = 1;
    public static final int SPEED_ENCODER_B = 2;
    public static final int PHI_LIMIT = 5;
    public static final int THETA_ENCODER_A = 6;
    public static final int THETA_ENCODER_B = 7;
    public static final int THETA_LIMIT = 8;
            //Relays
    public static final int RAMP_ARM = 1;

    //Joysticks
    public static final int DRIVE_STICK_1 = 1;
    public static final int DRIVE_STICK_2 = 2;
    public static final int PAYLOAD_STICK_1 = 3;
    public static final int PAYLOAD_STICK_2 = 4;

    //Analog
    public static final int GYRO = 1;

    //Solenoids
    public static final int LEFT_SOLENOID_ON = 1;
    public static final int LEFT_SOLENOID_OFF = 2;
    public static final int RIGHT_SOLENOID_ON = 3;
    public static final int RIGHT_SOLENOID_OFF = 4;
    public static final int SOLENOID_ON = 2;
    public static final int SOLENOID_OFF = 1;

    /*****************************************************************
    *                            PARAMETERS                          *
    *****************************************************************/

    // Autonomous Parameters
    public static final double AUTONOMOUS_SHOOT_DELAY = 3.0;
    public static final int AUTONOMOUS_PHI_SETPOINT = 65;
    public static final int AUTONOMOUS_SPEED_SETPOINT = 40000;
    
    public static final int AUTONOMOUS_MODES = 5;
    
    final public static int AUTONOMOUS_KINECT = 0;
    final public static int AUTONOMOUS_STILL = 1;
    final public static int AUTONOMOUS_FORWARD = 2;
    final public static int AUTONOMOUS_CENTER = 3;
    final public static int AUTONOMOUS_DISABLE = 4;
    
    //Drive Parameters
    public static final double DRIVE_PID_P = 0.0005;
    public static final double DRIVE_PID_I = 0.00015;
    public static final double DRIVE_PID_D = -0.00007;
    public static final int DRIVE_SHIFT_TRESHOLD_UPPER = 2500;
    public static final int DRIVE_SHIFT_TRESHOLD_LOWER = 2200;
    public static final boolean LOW_GEAR = false;
    public static final boolean HIGH_GEAR = true;

    // Payload Parameters
            // general constants
    public static final double GRAVITY = 32.0f * 12; // (per sec.)^2
    public static final double DEGREES_PER_RADIAN = 180.0f / (double) Math.PI;
    public static final double RADIANS_PER_DEGREE = (double) Math.PI / 180.0;
            // field measurements
    public static final int TARGET_COUNT = 4;
    public static final double HOOP_OFFSET = 15; //the wall to the middle of the hoop (in)
    public static final double DELTA_HEIGHT = 34; //how far the target is above the hoop (in)
    public static final double TARGET_WIDTH = 24; //vision target width(in)
    public static final double TARGET_HEIGHT = 18; //vision target height(in)
    public static final double TARGET_TOP_Y = 109;
    public static final double TARGET_MID_Y = 72;
    public static final double TARGET_BOT_Y = 39;
    public static final double TARGET_RHT_X = 31; //different for competition
    public static final double TARGET_MID_X = 0;
    public static final double TARGET_LFT_X = -31;
    public static final double TARGET_H_MAX_ERR = 10;
            // camera specs
    public static final int TARGET_COLOR_THRESHOLD_R_MIN = 0; //Target Color Minimum Thresholds
    public static final int TARGET_COLOR_THRESHOLD_G_MIN = 15;
    public static final int TARGET_COLOR_THRESHOLD_B_MIN = 50;
    public static final int TARGET_COLOR_THRESHOLD_R_MAX = 150; //Target Color Maximum Thresholds
    public static final int TARGET_COLOR_THRESHOLD_G_MAX = 34;
    public static final int TARGET_COLOR_THRESHOLD_B_MAX = 255;
    public static final double HORIZONTAL_FOV = 46.5;	//horizontal field of view TODO: measure these values for both cameras
    public static final double VERTICAL_FOV = 35.75;	//vertical field of view
            // camera position
    public static final double CAMERA_HEIGHT = 33;
    public static final double CAMERA_PERPENDICULAR_OFFSET = 0; //to be set once we know stuff from the CAD
    public static final double CAMERA_PARALLEL_OFFSET  = 0; // "        "          "           "
    public static final double CAMERA_VERTICAL_OFFSET = 0;
    public static final double HORIZONTAL_CAMERA_ANGLE_OFFSET = -10;	//degrees right that the camera is offset
    public static final double VERTICAL_CAMERA_ANGLE_OFFSET = 5;	//degrees up from horizontal that the camera points
    public static final double SWISHY_ONLY_RIGHT = (double) Math.PI / 6;
    public static final double SWISHY_ONLY_LEFT = 5 * (double) Math.PI / 6;
    public static final double DESIRED_ENTRY_ANGLE = (double) Math.PI / 4;
            // Belt Control
    public static final  double BELT_PICKUP_SPEED = 0.4;
    public static final double BELT_SHOOT_SPEED = 0.6;
    public static final double BELT_PICKUP_TIME = 0.1;
    public static final double BELT_SHOOT_TIME = 0.8;
    public static final int BELT_PICKUP_FORWARD = -1;
    public static final int BELT_PICKUP_REVERSE = 1;
    public static final int BELT_SHOOT_FORWARD = -1;
    public static final int BELT_SHOOT_REVERSE = 1;
            // Turret Control
    public static final double TURRET_WHEEL_DIAMETER = 6.5; //inches
    public static final double TURRET_WHEEL_CIRCUMFERENCE = (TURRET_WHEEL_DIAMETER * (double) Math.PI);
    public static final double TURRET_WHEEL_TOP_RATE = 26.1; //revs per second
    public static final double THETA_P = -0.001;
    public static final double THETA_I = -0.00005;
    public static final double THETA_D = -0.001;
    public static final double PHI_P = 0.01;
    public static final double PHI_I = 0.001;
    public static final double PHI_D = 0.0003;
    public static final double SPEED_P = 0.0001;
    public static final double SPEED_I = 0.00001;
    public static final double SPEED_D = 0;
    public static final double THETA_STEP = 16;
    public static final double THETA_DEADZONE = 1.6;
    public static final double THETA_MIN = -8000;
    public static final double THETA_MAX = 8000;
    public static final double PHI_STEP = 1;
    public static final double PHI_DEADZONE = 0.1;
    public static final double PHI_MAX = 140;
    public static final double PHI_ZERO_SPEED = -0.3;
    public static final double PHI_ZERO_TIME = 1.0;
    public static final double SPEED_STEP = 360;
    public static final double SPEED_DEADZONE = 36;
    public static final double SPEED_MAX = 40000;
    public static final double THETA_MOTOR = 0.5;
    public static final double PHI_MOTOR = 1;
    public static final double PHI_MOTOR_MAX = 1;
    public static final double THETA_PULSES_PER_DEGREE = 33;
    public static final double PHI_PULSES_PER_DEGREE = 47.0f / 16.0;
    public static final double PHI_MAX_ANGLE = 85;
            // Payload Control
    public static final double CONFIDENCE_THRESHOLD_DEFAULT = 0.5;
}
